
#ifndef _DRIVER_SPEED_CALCULATE_H
#define _DRIVER_SPEED_CALCULATE_H

#include "HeaderFiles.h"


#define ENCODER_LINE_NUM      940
#define SPEED_CAL_PERIOD_MS   5
#define WHEEL_SPEED_CAL_SF    (float32_t)((1000.0f / ENCODER_LINE_NUM) / SPEED_CAL_PERIOD_MS)

#if 0
#define MAGNET_ENCODER_LINE_NUM          2068
#define MAGNET_ENCODER_LINE_NUM_HALF     (MAGNET_ENCODER_LINE_NUM >> 1)
#define MAGNET_ENCODER_CALCULATE_SF      (float32_t)((1000.0f / MAGNET_ENCODER_LINE_NUM) / SPEED_CAL_PERIOD_MS)

#define MAGNET_ENCODER_ACCUM_MAX         (int32_t)(144177)
#define MAGNET_ENCODER_ACCUM_SF          (((float32_t) ENCODER_LINE_NUM) / ((float32_t) MAGNET_ENCODER_LINE_NUM))

#else

#define MAGNET_ENCODER_LINE_NUM          2972
#define MAGNET_ENCODER_LINE_NUM_HALF     (MAGNET_ENCODER_LINE_NUM >> 1)
#define MAGNET_ENCODER_CALCULATE_SF      (float32_t)((1000.0f / MAGNET_ENCODER_LINE_NUM) / SPEED_CAL_PERIOD_MS)

#define MAGNET_ENCODER_ACCUM_MAX         (int32_t)(65535)
#define MAGNET_ENCODER_ACCUM_SF          (float32_t)(1.0f)


#endif




typedef enum
{
	WHEEL_LIFT=0,
	WHEEL_RIGHT=1

}ENUM_WHEEL_NUM;



typedef struct
{
	uint16_t pluse_now;
	uint16_t pluse_old;
	
	int16_t  pluse_erro;
	
	float32_t real_rps;
	float32_t real_lpf_rps;

}STR_SPEED_CAL;


typedef struct
{
	uint8_t  motor_num;
	int16_t  eqep_now;
	int16_t  eqep_old;
	int16_t  eqep_erro;
	int32_t  eqep_accum;
	uint16_t eqep_accum_sf;

}STR_ENCODER_ACCUM;


extern uint16_t wheel_encoder_cnt[2];
extern uint16_t wheel_speed_pulse_cnt[2];
extern STR_ENCODER_ACCUM encoderAccum[2];

static inline uint16_t get_encoder_pulsecnt(ENUM_WHEEL_NUM num)
{
	return wheel_speed_pulse_cnt[num];
}

static inline uint16_t get_encoder_displacement(ENUM_WHEEL_NUM num)
{
	return wheel_encoder_cnt[num];
}


static inline uint16_t get_eqep_pulsecnt_left(void)
{
	return timer_counter_read(TIMER3);
}

static inline uint16_t get_eqep_pulsecnt_right(void)
{
	return (MAGNET_ENCODER_LINE_NUM - timer_counter_read(TIMER1));
}



extern void get_wheel_speed(void);
extern uint16_t get_encoder_accumulate(STR_ENCODER_ACCUM *hdl);
#endif
